#include "tools.h"
uint16_t crc16(uint8_t const *data, int32_t data_len, uint16_t poly)
{
    uint16_t crc_value = 0xFFFF;
    // reverse poly
    uint16_t poly_rev = 0;
    for (int i = 0; i < 16; i++)
    {
        if (poly & (1 << i))
            poly_rev |= (1 << (15 - i));
    }
    poly = poly_rev;
    while (data_len--)
    {
        crc_value ^= *data++;
        for (int i = 0; i < 8; i++)
            if (crc_value & 0x0001)
                crc_value = (crc_value >> 1) ^ poly;
            else
                crc_value >>= 1;
    }
    return crc_value;
}